import sys
import os
import time

from socf import conf
from socf.client import local
from socf.client import service
from socf.services.constants import *

from WSDiscovery import *

def __convertServiceListToMap(list):
    map = {}
    for entry in list:
        map[entry[2]] = entry[1].__str__()
    return map
    
mineList = []
boundary = "285"
workers_finished = None

def mineExists(mine):
    for m in mineList:
        if mine == m:
            return True
    return False
    
def addMine(mine):
    mineList.append(mine)
    
def getMineList():
    return mineList

def is_at_boundary(gps):
    loc = gps.getLocation()
    if loc[0] >= int(boundary):
        return True
    return False

def mine_detect():
    #creates configuration context
    configContext = conf.ConfigurationContext(os.path.join(os.getcwd(),  "conf_master"))
    #get service stub provider
    localProvider = local.LocalObjectStubProvider.createProvider(configContext, "dbus")
    #contact the discovery service
    discoveryStub = localProvider.getServiceClientStub(DISCOVERY_SERVICE_TYPE, "dbus:laf.socf1/server/discoveryService")
    servicesMap = __convertServiceListToMap(discoveryStub.getCurrentServicesList())
    
    nav = localProvider.getServiceClientStub(NAV_SERVICE_TYPE, servicesMap["Navigation_Simulated1"])
    sonar_front = localProvider.getServiceClientStub(SONAR_SERVICE_TYPE, servicesMap["Sonar_Simulated_Front1"])
    sonar_right = localProvider.getServiceClientStub(SONAR_SERVICE_TYPE, servicesMap["Sonar_Simulated_Right1"])
    compass = localProvider.getServiceClientStub(COMPASS_SERVICE_TYPE, servicesMap["Compass_Simulated1"])
    gps = localProvider.getServiceClientStub(GPS_SERVICE_TYPE, servicesMap["GPS_Simulated1"])
    md = localProvider.getServiceClientStub(METAL_DETECTOR_SERVICE_TYPE, servicesMap["MetalDetector_Simulated1"])
    
    nav.moveUp()
    print "running.."
    while 1:
        mines = md.getMines()
        if len(mines) > 0:
            for mine in mines:
                if not mineExists(mine):
                    addMine(mine)
                    print "Mine found at " + "[" + str(mine) + "]"
        if is_obstacle(sonar_front):
            if is_front(compass):
                nav.moveLeft()
                time.sleep(0.15)
                nav.moveLeft()
                if is_at_boundary(gps):
                    nav.stop()
                    break
            elif is_back(compass):
                if is_obstacle(sonar_right):
                    nav.stop()
                    break
                else:
                    nav.moveRight()
                    time.sleep(0.15)
                    nav.moveRight()
                    if is_at_boundary(gps):
                        nav.stop()
                        break
    
def is_front(compass):
    return compass.getDirection() == 0.0
    
def is_back(compass):
    return compass.getDirection() == 180.0
    
def is_obstacle(sonar):
    dist = sonar.getDistance()
    if dist < 3:
        return 1
    else:
        return 0

class MineMasterService:

    def mine_found(self,  mine,  source):
        """Notify a mine find.

##wsdl:xsd:string, xsd:string, xsd:string
"""
        
        if not mineExists(mine):
            global workers_finished
            if mine == "FINISHED":
                workers_finished = True
                print "worker " + str(source) + " is finished."
            else:
                addMine(mine)
                print "Mine " + "[" + str(mine) + "] found by " + str(source)
                
        return ""

if __name__=="__main__":
    #register master service
    mms = MineMasterService()
    ss = service.ServiceContainer("192.168.2.253", 8000)
    serviceUrl = ss.addServiceObject(mms,  "mine_master")
    ss.start()
    
    #discover workers
    print "\nsearching for workers.."
    wsd = WSDiscovery()
    wsd.start()
    type = QName("socf", "mineworker")
    services = wsd.searchServices(types=[type], timeout=3)
    if len(services) > 0:
        workers_finished = False
    wsd.stop()
    for entry in services:
        print "found worker at " + str(entry.getXAddrs()[0])
        service = service.ServiceClient(entry.getXAddrs()[0])
        service.registerMaster(serviceUrl,  boundary)
    
    try:
        #do my work
        mine_detect()        
        #wait for the workers
        if not workers_finished is None:
            while not workers_finished:
                time.sleep(0.2)                
        ss.stop()
        
        #show the list of mines
        print "List of mines in the area:-"
        print ", ".join(["[" + str(x) + "]" for x in getMineList()])
        
    except KeyboardInterrupt:
        print "\nShutdown.."        
        ss.stop()
    
